'''
Author: Jiagang Chen
Date: 2021-11-09 11:49:36
LastEditors: Jiagang Chen
LastEditTime: 2021-11-11 06:58:03
Description: 
Reference: 
'''
import numpy as np
import cv2
import math
import matplotlib.pyplot as plt

#高斯投影正算函数，返回高斯坐标:x,y
def LatLon2XY(latitude, longitude):
    a = 6378137.0
    # b = 6356752.3142
    # c = 6399593.6258
    # alpha = 1 / 298.257223563
    e2 = 0.0066943799013
    # epep = 0.00673949674227


    #将经纬度转换为弧度
    latitude2Rad = (np.pi / 180.0) * latitude

    beltNo = int((longitude + 1.5) / 3.0) #计算3度带投影度带号
    
    L = beltNo * 3 #计算中央经线
    
    l0 = longitude - L #经差
    tsin = np.sin(latitude2Rad)
    tcos = np.cos(latitude2Rad)
    t = np.tan(latitude2Rad)
    m = (np.pi / 180.0) * l0 * tcos
    et2 = e2 * pow(tcos, 2)
    et3 = e2 * pow(tsin, 2)
    X = 111132.9558 * latitude - 16038.6496 * np.sin(2 * latitude2Rad) + 16.8607 * np.sin(
        4 * latitude2Rad) - 0.0220 * np.sin(6 * latitude2Rad)
    N = a / np.sqrt(1 - et3)

    x = X + N * t * (0.5 * pow(m, 2) + (5.0 - pow(t, 2) + 9.0 * et2 + 4 * pow(et2, 2)) * pow(m, 4) / 24.0 + (
    61.0 - 58.0 * pow(t, 2) + pow(t, 4)) * pow(m, 6) / 720.0)
    y = 500000 + N * (m + (1.0 - pow(t, 2) + et2) * pow(m, 3) / 6.0 + (
    5.0 - 18.0 * pow(t, 2) + pow(t, 4) + 14.0 * et2 - 58.0 * et2 * pow(t, 2)) * pow(m, 5) / 120.0)

    return x, y

#高斯投影反算函数，返回大地坐标B,L
def XY2LatLon(X, Y, L0):

    iPI = 0.0174532925199433
    a = 6378137.0
    f= 0.00335281006247
    ZoneWide = 3 #按3度带进行投影

    ProjNo = int(Y / 1000000)
    L0 = L0 * iPI
    Y0 = ProjNo * 1000000 + 500000
    X0 = 0
    xval = X - X0
    yval = Y - Y0

    e2 = 2 * f - f * f #第一偏心率平方
    e1 = (1.0 - np.sqrt(1 - e2)) / (1.0 + np.sqrt(1 - e2))
    ee = e2 / (1 - e2) #第二偏心率平方

    M = xval
    u = M / (a * (1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256))

    fai = u \
          + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * np.sin(2 * u) \
          + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * np.sin(4 * u) \
          + (151 * e1 * e1 * e1 / 96) * np.sin(6 * u)\
          + (1097 * e1 * e1 * e1 * e1 / 512) * np.sin(8 * u)
    C = ee * np.cos(fai) * np.cos(fai)
    T = np.tan(fai) * np.tan(fai)
    NN = a / np.sqrt(1.0 - e2 * np.sin(fai) * np.sin(fai))
    R = a * (1 - e2) / np.sqrt(
        (1 - e2 * np.sin(fai) * np.sin(fai)) * (1 - e2 * np.sin(fai) * np.sin(fai)) * (1 - e2 * np.sin(fai) * np.sin(fai)))
    D = yval / NN

    #计算经纬度（弧度单位的经纬度）
    longitude1 = L0 + (D - (1 + 2 * T + C) * D * D * D / 6 + (
    5 - 2 * C + 28 * T - 3 * C * C + 8 * ee + 24 * T * T) * D * D * D * D * D / 120) / np.cos(fai)
    latitude1 = fai - (NN * np.tan(fai) / R) * (
    D * D / 2 - (5 + 3 * T + 10 * C - 4 * C * C - 9 * ee) * D * D * D * D / 24 + (
    61 + 90 * T + 298 * C + 45 * T * T - 256 * ee - 3 * C * C) * D * D * D * D * D * D / 720)

    #换换为deg
    longitude = longitude1 / iPI
    latitude = latitude1 / iPI

    return latitude, longitude


def cal_min_rect(coords):
    counter_list = []
    for pt in coords:
        pt_list = []
        pt_list.append([int(pt[0]+0.5), int(pt[1]+0.5)])
        counter_list.append(pt_list)
    # print(counter_list)
    counter = np.array(counter_list)
    # print(counter)
    rect = cv2.minAreaRect(counter)
    # print(rect)
    box_points = cv2.boxPoints(rect) 
    # print(box_points)
    # print(type(box_points))

    return [rect, box_points]


DEG2RAD = np.pi / 180.0
RAD2DEG = 180.0 / np.pi 


# 逆时间旋转 deg_angle
def rotation_points(deg_angle, center_pt, pt):
    normalized_point = [pt[0] - center_pt[0], pt[1] - center_pt[1]]
    rot_normalized_point = [math.cos(deg_angle * DEG2RAD) * normalized_point[0] - math.sin(deg_angle * DEG2RAD) * normalized_point[1], \
                            math.sin(deg_angle * DEG2RAD) * normalized_point[0] + math.cos(deg_angle * DEG2RAD) * normalized_point[1]]
    return [rot_normalized_point[0] + center_pt[0], rot_normalized_point[1] + center_pt[1]]


def plot_way_point(poly_pts, final_waterpoints):
    
    plt.figure()  # 创建绘制窗口

    for i in range(len(poly_pts)):
        pt1 = poly_pts[i%len(poly_pts)]
        pt2 = poly_pts[(i+1)%len(poly_pts)]
        x = [pt1[0], pt2[0]]
        y = [pt1[1], pt2[1]]
        plt.plot(x, y, color='black')
        plt.scatter(x, y, color='grey')

    for i in range(len(final_waterpoints)-1):
        pt1 = final_waterpoints[i%len(final_waterpoints)]
        pt2 = final_waterpoints[(i+1)%len(final_waterpoints)]
        x = [pt1[0], pt2[0]]
        y = [pt1[1], pt2[1]]
        plt.plot(x, y, color='r')
        plt.scatter(x, y, color='b')

    plt.show()


def get_waterpoint_from_rect(rect, poly_pts, range_x, overlap_x, range_y, overlap_y):
    dist_x = range_x * (1 - overlap_x)
    dist_y = range_y * (1 - overlap_y)

    origin_point = [(rect[0])[0] - (rect[1])[0] / 2.0, (rect[0])[1] - (rect[1])[1] / 2.0]
    num_x = int((rect[1])[0] // dist_x + 1)
    num_y = int((rect[1])[1] // dist_y + 1)
    
    init_waterpoints = []
    for y in range(num_y):
        if y % 2 == 0:
            for x in range(num_x):
                index = x
                wpt = [origin_point[0] + index * dist_x, origin_point[1] + y * dist_y]
                init_waterpoints.append(wpt)
        else:
            for x in range(num_x):
                index = num_x - x - 1
                wpt = [origin_point[0] + index * dist_x, origin_point[1] + y * dist_y]
                init_waterpoints.append(wpt)
    
    rot_poly_pts = []
    for pt in poly_pts:
        rot_poly_pts.append(rotation_points(-rect[2], [(rect[0])[0], (rect[0])[1]], pt))

    final_waterpoints = []
    for wpt in init_waterpoints:
        if point_in_region(wpt, rot_poly_pts) \
            or point_in_region([wpt[0] - range_x / 2.0, wpt[1] - range_y / 2.0], rot_poly_pts) \
            or point_in_region([wpt[0] - range_x / 2.0, wpt[1] + range_y / 2.0], rot_poly_pts) \
            or point_in_region([wpt[0] + range_x / 2.0, wpt[1] - range_y / 2.0], rot_poly_pts) \
            or point_in_region([wpt[0] + range_x / 2.0, wpt[1] + range_y / 2.0], rot_poly_pts):
            final_waterpoints.append(rotation_points(rect[2], [(rect[0])[0], (rect[0])[1]], wpt))

    plot_way_point(poly_pts, final_waterpoints)

    return final_waterpoints
    

def point_in_region(pt, line_pt):
    nCross = 0;

    for i in range(len(line_pt)):

        p1 = line_pt[i];
        p2 = line_pt[(i+1)%len(line_pt)];

        if p1[1] == p2[1]:
            continue

        if pt[1] < min(p1[1], p2[1]):
            continue;

        if pt[1] >= max(p1[1], p2[1]):
            continue

        x = (pt[1] - p1[1]) * (p2[0] - p1[0]) / (p2[1] - p1[1]) + p1[0]

        if x > pt[0]:
            nCross = nCross + 1

    if nCross % 2 == 1:
        return True
    else:
        return False

if __name__ == "__main__":
    print("OKK")

    